home
***
CD-ROM
|
disk
|
FTP
|
other
***
search
/
Power Programmierung
/
Power-Programmierung (Tewi)(1994).iso
/
assemblr
/
library
/
robot
/
robotasm.new
< prev
Wrap
Text File
|
1991-07-09
|
20KB
|
323 lines
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ROBOT MOTION PROGRAM 1.00.01 ;
; ;
; REVISION 7-8-90 ;
; ;
; CHANGED LAST ON 3-7-91 ;
; 7-09-91 ADDED TURNDIST TO ALLOW CONTROL OF ROTATION ON TURNS ;
; 3-7-91 CHANGED SPEEDCOR TO REFLECT 6 MHZ CRYSTAL ;
; ;
; ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; INITIALIZE MEMORY LOCATIONS ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
HEAD .EQU 32H ;HEAD VALUE OF FORWARD TABLE
TAIL .EQU 35H ;TAIL VALUE OF FORWARD TABLE
RIGHTHEAD .EQU 36H ;HEAD VALUE OF RIGHT STOPPED TABLE
RIGHTTAIL .EQU 39H ;TAIL VALUE OF RIGHT STOPPED TABLE
LEFTHEAD .EQU 3AH ;HEAD VALUE OF LEFT STOPPED TABLE
LEFTTAIL .EQU 3DH ;TAIL VALUE OF LEFT STOPPED TABLE
STEPRIGHT .EQU 3EH ;RIGHT MOTOR STEP COUNT
STEPLEFT .EQU 3FH ;LEFT MOTOR STEP COUNT
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; UNIVERSAL CONSTANTS ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;SPEEDCOR .EQU 48H ;COARSE SPEED ADJUSTMENT FOR 8 MHZ
SPEEDCOR .EQU 36H ;COARSE SPEED ADJUSTMENT FOR 6 MHZ
SPEEDFIN .EQU 0FFH ;FINE SPEED ADJUSTMENT
REVDIST .EQU 30H ;DISTANCE COUNT TO BACKUP
TURNDIST .EQU 18H ;DISTANCE TO TURN
STEPCOUNT .EQU 18H ;STEPS BETWEEN SENSOR TOGGLES
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; PROGRAM START ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
.ORG 000H
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; INITIALIZE PORT 2 FOR INPUT ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MOV A,#0FFH ;MAKE PORT 2 AN INPUT PORT
OUTL P2,A ;INITIALIZE PORT 2 FOR INPUT
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; BUILD THE FORWARD ROTATE TABLE IN MEMORY ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MOV R0,#HEAD ;POINT TO HEAD OF ROTATE TABLE
MOV A,#01010101B ;LOAD ACCUMULATOR WITH HEAD ENTRY
MOV @R0,A ;STORE THE HEAD ENTRY
INC R0 ;POINT TO HEAD + 1
MOV A,#10011001B ;LOAD ACCUMULATOR WITH HEAD + 1 ENTRY
MOV @R0,A ;STORE THE ENTRY
INC R0 ;POINT TO HEAD + 2
MOV A,#10101010B ;LOAD ACC WITH HEAD + 2 ENTRY
MOV @R0,A ;STORE THE ENTRY
INC R0 ;POINT TO TAIL
MOV A,#01100110B ;LOAD ACC WITH TAIL ENTRY
MOV @R0,A ;STORE THE ENTRY
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; BUILD THE RIGHT STOPPED ROTATE TABLE IN MEMORY ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MOV R0,#RIGHTHEAD ;POINT TO HEAD OF RIGHT STOPPED ROTATE TABLE
MOV A,#01010101B ;LOAD ACCUMULATOR WITH HEAD ENTRY
MOV @R0,A ;STORE THE HEAD ENTRY
INC R0 ;POINT TO HEAD + 1
MOV A,#01101001B ;LOAD ACCUMULATOR WITH HEAD + 1 ENTRY
MOV @R0,A ;STORE THE ENTRY
INC R0 ;POINT TO HEAD + 2
MOV A,#10101010B ;LOAD ACC WITH HEAD + 2 ENTRY
MOV @R0,A ;STORE THE ENTRY
INC R0 ;POINT TO TAIL
MOV A,#10010110B ;LOAD ACC WITH TAIL ENTRY
MOV @R0,A ;STORE THE ENTRY
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; BUILD THE LEFT STOPPED ROTATE TABLE IN MEMORY ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MOV R0,#LEFTHEAD ;POINT TO HEAD OF LEFT STOPPED ROTATE TABLE
MOV A,#01010101B ;LOAD ACCUMULATOR WITH HEAD ENTRY
MOV @R0,A ;STORE THE HEAD ENTRY
INC R0 ;POINT TO HEAD + 1
MOV A,#10010110B ;LOAD ACCUMULATOR WITH HEAD + 1 ENTRY
MOV @R0,A ;STORE THE ENTRY
INC R0 ;POINT TO HEAD + 2
MOV A,#10101010B ;LOAD ACC WITH HEAD + 2 ENTRY
MOV @R0,A ;STORE THE ENTRY
INC R0 ;POINT TO TAIL
MOV A,#01101001B ;LOAD ACC WITH TAIL ENTRY
MOV @R0,A ;STORE THE ENTRY
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; INITIALIZE FORWARD ROTATE TABLE POINTER ;
; INITIALIZE MOTOR STEP COUNTS ;
; INITIALIZE LEFT AND RIGHT MOTOR STATUS ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
MOV R0,#HEAD ;POINT TO HEAD OF ROTATE TABLE
CALL SETRIGHT ;INITIALIZE LEFT AND RIGHT MOTOR STEP COUNTS
CALL SETLEFT ;
IN A,P2 ;GET INITIAL MOTOR STATUS
MOV R5,A ;TEMPORARILY STORE ACC IN R5
ANL A,#00000001B ;MASK FOR RIGHT MOTOR STATUS
MOV R4,A ;LOAD R4 WITH RIGHT INITIAL MOTOR STATUS
MOV A,R5 ;RESTORE RIGHT AND LEFT MOTOR STATUS READINGS
ANL A,#00000010B ;MASK FOR LEFT MOTOR STATUS
MOV R3,A ;LOAD R3 WITH LEFT INITIAL MOTOR STATUS
SEEK MOV A,@R0 ;LOAD ACC WITH HEAD VALUE
OUTL P1,A ;LOAD THE TABLE VALUE TO THE MOTOR DRIVER
CALL STATUS ;LOAD MOTOR DRIVER WITH HEAD VALUE
INC R0 ;POINT THE HEAD + 1 VALUE
MOV A,#TAIL+1 ;CHECK R0 FOR END OF TABLE
XRL A,R0 ;COMPARE R0 WITH VALUE IN ACC
JNZ SEEK ;NOT BEYOND THE TAIL CONTINUE TO MOVE FORWARD
MOV R0,#HEAD ;RESET R0 TO HEAD VALUE
JMP SEEK
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; SUBROUTINES ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
.ORG 100H
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; MOVE STATUS SUBROUTINE ;
; ;
; CHANGE VALUES FOR R5 AND R7 TO AFFECT SPEED OF STEPPER MOTORS... ;
; R0 = FORWARD TABLE POINTER ;
; R1 = STEP COUNTER POINTER ;
; R2 = REVERSE ROTATION COUNTER ;
; R3 = PREVIOUS MOTOR STATUS LEFT MOTOR ;
; R4 = PREVIOUS MOTOR STATUS RIGHT MOTOR ;
; R5 = SCRATCH REGISTER ;
; R6 = OUTER LOOP COUNT VALUE (COARSE ADJUSTMENT) ;
; R7 = INNER LOOP COUNT VALUE (FINE ADJUSTMENT) ;
; P1 = STEPPER MOTOR PORT BITS 7-4 = LEFT MOTOR BITS 3-0 = RIGHT MOTOR ;
; P2 = MOTOR STATUS ;
; F0 = RIGHT MOTION FLAG - SET WHEN RIGHT WHEEL IS STOPPED ;
; F1 = LEFT MOTION FLAG - SET WHEN LEFT WHEEL IS STOPPED ;
; ;
; P2 BIT 7 6 5 4 3 2 1 0 ;
; N/A N/A N/A N/A N/A N/A LEFT MOTOR RIGHT MOTOR ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
STATUS IN A,P2 ;GET CURRENT MOTOR STATUS
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; CHECK THE RIGHT WHEEL MOTION ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CHECKRIGHT CLR F0 ;CLEAR THE FLAGS
CLR F1 ;
CPL F0 ;SET THE FLAGS
CPL F1 ;ROUTINE MUST CLEAR FLAGS TO SHOW MOVEMENT
MOV R5,A ;TEMPORARILY STORE CURRENT MOTOR STATUS
ANL A,#00000001B ;MASK OUT LEFT MOTOR STATUS VALUE
XRL A,R4 ;COMPARE RIGHT MOTOR STATUS
JNZ RESETRIGHT ;IF NOT SAME AS PREVIOUS THEN RESET STEP COUNT
DECRIGHT MOV R1,#STEPRIGHT ;POINT TO RIGHT MOTOR STEP COUNT
MOV A,@R1 ;GET THE STEPRIGHT VALUE FOR DECREMENT
DEC A ;DECREMENT THE STEPLEFT VALUE
MOV @R1,A ;STORE THE NEW RIGHT MOTOR STEP COUNT
MOV A,#00H ;LOAD ACC WITH 0
XRL A,@R1 ;CHECK STEPRIGHT VALUE FOR 0
JZ REVERSE ;IF STEP COUNT = 0 THEN BACK UP
JMP CHECKLEFT ;GO CHECK THE LEFT MOTOR STATUS
RESETRIGHT MOV A,R5 ;RESTORE THE PREVIOUS RIGHT AND LEFT STATUS
ANL A,#00000001B ;MASK OUT LEFT MOTOR STATUS
MOV R4,A ;STORE THE NEW RIGHT MOTOR STATUS
CALL SETRIGHT ;GO RESET THE RIGHT MOTOR COUNT
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; CHECK THE LEFT WHEEL MOTION ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
CHECKLEFT CLR F0 ;CLEAR FLAG 0 TO SHOW RIGHT WHEEL WAS OK
MOV A,R5 ;RESTORE THE CURRENT MOTOR STATUS
ANL A,#00000010B ;MASK OUT RIGHT MOTOR STATUS VALUE
XRL A,R3 ;COMPARE LEFT MOTOR STATUS
JNZ RESETLEFT ;IF NOT SAME AS PREVIOUS THEN RESET STEP COUNT
DECLEFT MOV R1,#STEPLEFT ;POINT TO LEFT MOTOR STEP COUNT
MOV A,@R1 ;GET THE STEPLEFT VALUE FOR DECREMENT
DEC A ;DECREMENT THE STEPLEFT VALUE
MOV @R1,A ;STORE THE NEW LEFT MOTOR STEP COUNT
MOV A,#00H ;LOAD ACC WITH 0
XRL A,@R1 ;CHECK STEPLEFT VALUE FOR 0
JZ REVERSE ;IF STEP COUNT = 0 THEN BACK UP
JMP STEPWAIT ;GO WAIT UNTIL TIME FOR THE NEXT STEP
RESETLEFT MOV A,R5 ;RESTORE THE PREVIOUS RIGHT AND LEFT STATUS
ANL A,#00000010B ;MASK OUT RIGHT MOTOR STATUS
MOV R3,A ;STORE THE NEW LEFT MOTOR STATUS
CALL SETLEFT ;GO RESET THE LEFT MOTOR COUNT
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; DELAY BETWEEN STEPPER MOTOR STEPS ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
STEPWAIT CLR F1 ;CLEAR FLAG TO SHOW LEFT WHEEL WAS MOVING
MOV R7,#SPEEDCOR ;WAIT BETWEEN STEPS (OUTER LOOP COUNTER)
WAITING MOV R5,#SPEEDFIN ;WAIT BETWEEN STEPS (INNER LOOP COUNTER)
DJNZ R5,$ ;DECREMENT THE INNER LOOP COUNTER
DJNZ R7,WAITING ;DECREMENT THE OUTER LOOP COUNTER
MOVEDONE RET
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; REVERSE AND TURN ROUTINES ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
REVERSE CALL BACKITUP ;A WHEEL HAS STOPPED SO BACK IT UP
JF1 LEFTSTOP ;IF FLAG 1 IS SET, LEFT WHEEL HAS STOPPED
JF0 RIGHTSTOP ;IF FLAG 0 IS SET, RIGHT WHEEL HAS STOPPED
LEFTSTOP MOV R0,#LEFTHEAD ;POINT TO START OF LEFT STOPPED ROTATE TABLE
JMP TURN
RIGHTSTOP MOV R0,#RIGHTHEAD ;POINT TO BEGINNING OF RIGHT STOPPED ROTATE TABLE
TURN MOV R2,#TURNDIST ;LOAD R2 WITH TURN DISTANCE COUNT
BEGINTURN MOV A,@R0 ;LOAD ACC WITH HEAD VALUE
OUTL P1,A ;LOAD THE TABLE VALUE TO THE MOTOR DRIVER
MOV R7,#SPEEDCOR ;WAIT BETWEEN STEPS (OUTER LOOP COUNTER)
TURNING MOV R5,#SPEEDFIN ;WAIT BETWEEN STEPS (INNER LOOP COUNTER)
DJNZ R5,$ ;DECREMENT THE INNER LOOP COUNTER
DJNZ R7,TURNING ;DECREMENT THE OUTER LOOP COUNTER
INC R0 ;POINT THE HEAD + 1 VALUE
DEC R2 ;DECREMENT THE TURN DISTANCE COUNT
MOV A,R2 ;IS TURN DONE?
JZ TURNDONE ;YES..TURN IS DONE
JF1 TAILLEFT ;NO..DETERMINE WHICH TURN TABLE WE ARE IN
MOV A,#RIGHTTAIL+1 ;WE ARE IN RIGHT STOPPED TABLE
JMP COMPARE ;GO FIND OUR LOCATION IN THE TURN TABLE
TAILLEFT MOV A,#LEFTTAIL+1 ;WE ARE IN LEFT STOPPED TABLE
COMPARE XRL A,R0 ;DETERMINE POSITION IN OUR TURN TABLE
JNZ BEGINTURN ;NOT BEYOND THE TAIL CONTINUE TO TURN
JF1 HEADLEFT ;BEYOND TAIL..DETERMINE WHICH POINTER TO RESET
MOV R0,#RIGHTHEAD ;RESET THE RIGHT STOPPED TABLE POINTER
JMP BEGINTURN ;CONTINUE TO TURN
HEADLEFT MOV R0,#LEFTHEAD ;RESET THE LEFT STOPPED TABLE POINTER
JMP BEGINTURN ;CONTINUE TO TURN
TURNDONE MOV R0,#HEAD ;RESET THE POINTER TO FORWARD ROTATE TABLE
JMP MOVEDONE ;GET OUT OF THIS SUBROUTINE
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; REVERSE SUBROUTINE ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
BACKITUP CALL SETRIGHT ;RESET THE MOTOR COUNTERS
CALL SETLEFT
MOV R2,#REVDIST ;LOAD THE REVERSE DISTANCE VALUE
GOBACK DEC R0 ;REVERSE MODE - MOVE BACKWARDS IN ROTATE TABLE
MOV A,#HEAD-1 ;CHECK R0 FOR END OF TABLE
XRL A,R0 ;COMPARE R0 WITH VALUE IN ACC
JNZ BACKUP ;NOT BEYOND TABLE LIMITS-CONTINUE TO MOVE
MOV A,#TAIL ;RESET R0 TO TAIL VALUE
MOV R0,A
BACKUP MOV A,@R0 ;LOAD ACC WITH TABLE VALUE
OUTL P1,A ;LOAD TABLE VALUE TO STEPPER MOTORS
BACKWAIT MOV R7,#SPEEDCOR ;WAIT BETWEEN STEPS (COARSE VALUE)
WAITBACK MOV R5,#SPEEDFIN ;WAIT BETWEEN STEPS (FINE VALUE)
DJNZ R5,$ ;DECREMENT THE INNER LOOP COUNTER
DJNZ R7,WAITBACK ;DECREMENT THE OUTER LOOP COUNTER
DEC R2
MOV A,R2
JZ BACKUPDONE
JMP GOBACK
BACKUPDONE RET
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; ;
; MOTOR SWITCH TOGGLE RATE INITIALIZATION SUBROUTINE ;
; ;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
SETRIGHT MOV R1,#STEPRIGHT ;POINT TO RIGHT MOTOR STEP COUNTER
MOV A,#STEPCOUNT ;LOAD A WITH 24 STEPS
MOV @R1,A ;RELOAD RIGHT MOTOR STEP COUNTER TO 24 STEPS
RET
SETLEFT MOV R1,#STEPLEFT ;POINT TO LEFT MOTOR STEP COUNTER
MOV A,#STEPCOUNT ;LOAD A WITH 24 STEPS
MOV @R1,A ;RELOAD LEFT MOTOR STEP COUNTER TO 24 STEPS
RET
.END
